package com.yuneec.rcdronesercive;

import com.yuneec.droneservice.DroneService;
import com.yuneec.droneservice.command.BaseCommand;
import com.yuneec.droneservice.command.MAV_RC_COMMAND;
import com.yuneec.droneservice.model.HardwareEvent;
import com.yuneec.droneservice.parse.DroneFeedback;
import com.yuneec.droneservice.protocol.ProtocolPlugin;
import com.yuneec.droneservice.request.IRequest;
import com.yuneec.droneservice.request.IResponseHandler;
import com.yuneec.droneservice.request.OnListener;
import com.yuneec.droneservice.request.OnListenerHandler;
import com.yuneec.droneservice.utils.ByteUtils;

import java.lang.ref.WeakReference;
import java.util.Arrays;

/**
 * Created by zd.zhang on 2017/9/11.
 */

public class RCProtocolPlugin implements ProtocolPlugin {
    public static final int BASE_COMMAND_MSG_ID = 1108;
    private final boolean isBigEndian = BaseCommand.IS_BIGENDIAN;
    ///////////////////////////////////////////////////////////////////
    /////////////////////////////////
    private OnListenerHandler<DroneFeedback> droneFeedbackListener;
    private OnListenerHandler<HardwareEvent> hardwareEventListener;
    private OnListenerHandler<byte[]> payloadByPassListener;
    private WeakReference<DroneService.DroneBinder> binderReference;
    /////////////////////////////////////////////////////////////////////

    @Override
    public IResponseHandler handleResponseBuf(byte[] data, int offset, int length) {
        byte senderId = data[offset + 1];
        int payloadOff = offset + 4;
        int commandId = data[payloadOff] & 0xFF;
        if (senderId == BaseCommand.RESPONSE_SENDERID || senderId == BaseCommand.RESPONSE_RADIO) {
            switch (commandId) {
                case MAV_RC_COMMAND.MAV_RC_MSG_TELEMETRY_DATA:
                    return handleDroneFeedback(data, offset, length);
                case MAV_RC_COMMAND.MAV_RC_MSG_BYPASS_EX_PACKET:
                    return handPayloadByPass(data, offset, length);
                case MAV_RC_COMMAND.MAV_RC_MSG_REPORT_EVENT:
                    return handControllerEvent(data, offset, length);
                default:
                    return new RCIRH(commandId, data, offset, length);
            }
        }
        return null;
    }

    @Override
    public byte[] toRawData(IRequest request) {
        byte[] buf = null;
        if (request != null) {
            buf = request.toRawData();
        }
        if (buf == null) {
            buf = new byte[0];
        }
        return buf;
    }

    @Override
    public int getPort() {
        return BASE_COMMAND_MSG_ID;
    }

    @Override
    public void init(DroneService.DroneBinder mBinder) {
        binderReference = new WeakReference<>(mBinder);
    }

    private IResponseHandler handleDroneFeedback(byte[] data, int off, int len) {
        if (droneFeedbackListener == null) {
            droneFeedbackListener = new OnListenerHandler<>(OnListener.L_DRONE_FEEDBACK, new DroneFeedback(), binderReference.get());
        }
        DroneFeedback droneFeedback = droneFeedbackListener.get();
        droneFeedback.latitude = ByteUtils.byteArrayToInt(Arrays.copyOfRange(data, off + 4, off + 8), BaseCommand.IS_BIGENDIAN) * 1e-7f;
        droneFeedback.longitude = ByteUtils.byteArrayToInt(Arrays.copyOfRange(data, off + 8, off + 12), BaseCommand.IS_BIGENDIAN) * 1e-7f;
        droneFeedback.altitude = ByteUtils.byteArrayToInt(Arrays.copyOfRange(data, off + 12, off + 16), BaseCommand.IS_BIGENDIAN) * 0.01f;
        float vx = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 16, off + 18), BaseCommand.IS_BIGENDIAN) * 0.01f;
        float vy = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 18, off + 20), BaseCommand.IS_BIGENDIAN) * 0.01f;
        float vz = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 20, off + 22), BaseCommand.IS_BIGENDIAN) * 0.01f;
        droneFeedback.tas = (float) Math.sqrt(vx * vx + vy * vy + vz * vz);
        droneFeedback.vSpeed = vz;
        droneFeedback.hSpeed = (float) Math.sqrt(vx * vx + vy * vy);
        droneFeedback.gpsUsed = ((data[off + 22] & 0x80) >> 7) > 0;
        droneFeedback.fixType = ((data[off + 22] & 0x60)) >> 5;
        droneFeedback.satellitesNum = data[off + 22] & 0x1f;
        droneFeedback.voltage = (data[off + 23] & 0xff) * 0.1f + 5;
        droneFeedback.current = (data[off + 24] & 0xff) * 0.5f;
        droneFeedback.roll = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 25, off + 27), BaseCommand.IS_BIGENDIAN) * 0.01f;
        droneFeedback.pitch = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 27, off + 29), BaseCommand.IS_BIGENDIAN) * 0.01f;
//        droneFeedback.yaw = normalizeYaw(ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, 29, 31), BaseCommand.IS_BIGENDIAN) * 0.01f);
        droneFeedback.yaw = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 29, off + 31), BaseCommand.IS_BIGENDIAN) * 0.01f;
        droneFeedback.motorStatus = data[off + 31] & 0xff;
        droneFeedback.imuStatus = data[off + 32] & 0xff;
//        droneFeedback.gpsStatus = (data[32] & 0x20) >> 5;
        droneFeedback.pressCompassStatus = data[off + 33] & 0xff;
        droneFeedback.fMode = data[off + 34] & 0xff & 0x7f;
        droneFeedback.vehicleType = data[off + 35] & 0xff;
//        droneFeedback.cgpsStatus = droneFeedback.imuStatus >> 6;
//        droneFeedback.gpsPosUsed = (droneFeedback.fMode >> 7) > 0;
//            }
        droneFeedback.errorFlags1 = data[off + 36] & 0xff;
        droneFeedback.gpsAccH = (data[off + 37] & 0xff) * 0.05f;
        droneFeedback.fskRssi = data[off + 38];
        return droneFeedbackListener;
    }

//    public static float normalizeYaw(float yaw) {
//        float rtn = 0;
//
//        if (yaw < 0) {
//            yaw += 360;
//        }
//
//        if (yaw >= 0 && yaw <= 90) {
//            rtn = 90 - yaw;
//        } else if (yaw > 90 && yaw <= 360) {
//            rtn = 450 - yaw;
//        }
//        return rtn;
//    }

    private IResponseHandler handPayloadByPass(byte[] payload, int off, int len) {
        if (payloadByPassListener == null) {
            DroneService.DroneBinder binder = binderReference.get();
            if (binder != null) {
                payloadByPassListener = new OnListenerHandler<>(OnListener.L_PAYLOAD_BY_PASS, null, binder);
            } else {
                return null;
            }
        }
        byte[] buf = Arrays.copyOfRange(payload, off + 2, payload.length - 2);
        payloadByPassListener.set(buf);
        return payloadByPassListener;
    }

    private IResponseHandler handControllerEvent(byte[] payload, int off, int len) {
        if (hardwareEventListener == null) {
            DroneService.DroneBinder binder = binderReference.get();
            if (binder == null) {
                return null;
            }
            hardwareEventListener = new OnListenerHandler<>(OnListener.L_CONTROLLER_FEEDBACK, new HardwareEvent(), binder);
        }
        HardwareEvent hardwareEvent = hardwareEventListener.get();
        hardwareEvent.setHwId(payload[off + 6] & 0xff);
        hardwareEvent.setValue(payload[off + 7] & 0xff);
        return hardwareEventListener;
    }

}
